#include "eigen3/Eigen/Dense"
#include <iostream>
#include "opencv2/opencv.hpp"

using namespace std;

void point_inside_polygon(float* pts,int pts_num, float* polygon_vertices,int polygon_side_num,int polygon_num,
                        bool * is_in_box )
{
    for(int q=0;q<polygon_num;q++)
    {
        //循环第i点，判断是否在polygon中(polygon_side_num=4, 四边形)，更新is_in_box
        printf("polygon num %d \n",q);
        for (int i=0;i<pts_num;i++)
        {
            float x = pts[i*2+0];
            float y = pts[i*2+1];
            // printf("i %d pts x,y  %f %f \n",i,x,y);

            int j = polygon_side_num-1;
            for(int k=0;k<polygon_side_num;k++)
            {
                float xk = polygon_vertices[q*8+2*k+0]; //每个polygon 4个顶点 8 个值
                float yk = polygon_vertices[q*8+2*k+1];

                float xj = polygon_vertices[q*8+2*j+0];
                float yj = polygon_vertices[q*8+2*j+1];

                // printf("vertices pts k x,y %f %f \n",xk,yk);
                // printf("vertices pts j x,y %f %f \n",xj,yj);
                // 这里必须4此循环
                bool is_mid_y = (yk<y && y<=yj) || ( yj < y && y <= yk);
                if(is_mid_y)
                {
                    // printf("is_mid_y pts %f %f \n",x, y);
                    bool is_mid_x = x > ((xj - xk) * (y - yk) / (yj - yk) + xk);
                    if (is_mid_x)
                    {
                        // printf("is_mid_x %f \n",(xj - xk) * (y - yk) / (yj - yk) + xk);
                        is_in_box[q*pts_num+i]=!is_in_box[q*pts_num+i];
                    }
                }
                j=k;
            }

            if(is_in_box[q*pts_num+i])
                printf("in box %d \n",is_in_box[q*pts_num+i]);
        }
    }
}

void point_inside_polygon_gpu(float* pts,int pts_num, float* polygon_vertices,int polygon_vertices_nu,
                        bool * is_in_box )
{


}


void get_pts_and_vertices(float * polygon_vertices_f,int polygon_num,cv::Mat &im ) // ,float*pts_f,int pts_num 
{
    
    //draw vertices
    vector<Eigen::Matrix<int,4,2>> polygon_vertices_list{2};
    polygon_vertices_list[0] << 1, 1,
                         4, 2,
                        3, 4,
                         2, 4;
    polygon_vertices_list[0]*=100;

    polygon_vertices_list[1] << 2, 1,
                         4, 3,
                        3, 5,
                         1, 4;
    polygon_vertices_list[1]*=100;
    
    // cout << polygon_vertices_list[0] << endl;
    // cout << polygon_vertices_list[1] << endl;

    //need copy Matrix to float[]
    for(int q_ind=0;q_ind<polygon_num;q_ind++)
    {
        Eigen::Matrix<int,4,2> polygon_vertices = polygon_vertices_list[q_ind];
        for(int i=0;i<polygon_vertices.rows();i++)
        {
            cv::Scalar blue = cv::Scalar(255,0,0);
            cv::circle(im,cv::Point(polygon_vertices(i,0),polygon_vertices(i,1)),1,blue,1,1);
            polygon_vertices_f[q_ind*8+i*2+0]=polygon_vertices(i,0);
            polygon_vertices_f[q_ind*8+i*2+1]=polygon_vertices(i,1);

            //draw line
            cv::Point p1 =cv::Point(polygon_vertices(i,0),polygon_vertices(i,1));
            cv::Point p2;
            if (i+1==polygon_vertices.rows())
                p2 =cv::Point(polygon_vertices(0,0),polygon_vertices(0,1));
            else
                p2 =cv::Point(polygon_vertices(i+1,0),polygon_vertices(i+1,1));
            cv::line(im,p1,p2,blue,1,1);
        }
    }

    //draw pts
    // Eigen::Matrix<int,5,2> pts;
    // pts << 2, 2, 
    //         3, 3, 
    //         3, 2,
    //         4, 3,
    //         2, 4;
    // pts*=100;

    // for(int i=0;i<pts.rows();i++)
    // {
    //     cv::Scalar red = cv::Scalar(0,0,255);
    //     cv::circle(im,cv::Point(pts(i,0),pts(i,1)),1,red,1,1);
    //     pts_f[2*i+0]=pts(i,0);
    //     pts_f[2*i+1]=pts(i,1);
    // }
}

int main() {
    //frame draw
    cv::Mat im=cv::Mat::zeros(cv::Size(500,500),CV_8UC3);
    //pts,按照 xrang(0,500),yrang(0,500),voxel_size=10,grid_size = 500/10 ^2 = 250 
    int pts_num=50*50;
    float *pts_f=new float[2*pts_num];
    int pts_ind= 0;
    for(int i=0;i<500;i=i+10)
    {
        for (int j=0;j<500;j=j+10)
        {
            pts_f[pts_ind*2+0]=i+5;
            pts_f[pts_ind*2+1]=j+5;
            // cv::Scalar red = cv::Scalar(0,0,255);
            // cv::circle(im,cv::Point(i+5,j+5),1,red,1,1);
            pts_ind++;
        }
    }
    // cv::imwrite("res.jpg",im);
    // return 0;

    int polygons_num = 2;
    int polygon_side_num=4; //其实默认都是4边形
    float *polygon_vertices_f = new float[polygons_num*polygon_side_num*2];
    get_pts_and_vertices(polygon_vertices_f,polygons_num,im);//,pts_f,pts_num,im);
    // return 0;

    //cpu
    //存放每个点在每个box内的 flag,默认都不在
    bool *is_in_box = new bool[pts_num*polygons_num]{0};
    point_inside_polygon(pts_f,pts_num,polygon_vertices_f,polygon_side_num,polygons_num,is_in_box);
    //gpu
    // point_inside_polygon_gpu(pts_f,pts_num,polygon_vertices_f,4,is_in_box);

    //draw in box is green, out is red
    for(int i=0;i<pts_num;i++)
    {
        cv::Scalar red = cv::Scalar(0,0,255);
        cv::Scalar green = cv::Scalar(0,255,0);
        //判断同时在2个polygon内
        // printf("is in 1st box and 2nd box %d %d \n ",is_in_box[pts_num*0+i],is_in_box[pts_num*1+i]);
        if (is_in_box[pts_num*0+i]==1 || is_in_box[pts_num*1+i]==1)
            cv::circle(im,cv::Point(pts_f[i*2+0],pts_f[i*2+1]),1,green,1,1);
        else
        {
            cv::circle(im,cv::Point(pts_f[i*2+0],pts_f[i*2+1]),1,red,1,1);
        }
    }

    cv::flip(im,im,0);
    cv::imwrite("res.jpg",im);
    return 0;
}
